#include <vector3.h>
#include <unittest.h>
#include <debug.h>

/**
 * DESC: Test vector3 operations
 *
 * IMP: HIGH
 */

int main( int argc, char *argv[] )
{
  debug_level = DEBUG_ERR;

  vector3_t i = vector3_init( 1.0, 0.0, 0.0 );
  vector3_t j = vector3_init( 0.0, 1.0, 0.0 );
  vector3_t k = vector3_init( 0.0, 0.0, 1.0 );

  vector3_t i2 = vector3_init( 2.0, 0.0, 0.0 );
  vector3_t i3 = vector3_init( 3.0, 0.0, 0.0 );

  vector3_t d_ij = vector3_init( 1.0, 1.0, 0.0 );
  vector3_t d_ik = vector3_init( 1.0, 0.0, 1.0 );
  vector3_t d_jk = vector3_init( 0.0, 1.0, 1.0 );
  vector3_t d    = vector3_init( 1.0, 1.0, 1.0 );

  assert_equal_int( vector3_is_equal( i, i ), 1 );
  assert_equal_int( vector3_is_equal( j, j ), 1 );
  assert_equal_int( vector3_is_equal( k, k ), 1 );

  assert_equal_int( vector3_is_perpendicular( i, j ), 1 );
  assert_equal_int( vector3_is_perpendicular( i, k ), 1 );
  assert_equal_int( vector3_is_perpendicular( k, j ), 1 );

  assert_equal_int( vector3_is_perpendicular( i, i ), 0 );
  assert_equal_int( vector3_is_perpendicular( j, j ), 0 );
  assert_equal_int( vector3_is_perpendicular( k, k ), 0 );

  assert_equal_int( vector3_is_parallel( i, j ), 0 );
  assert_equal_int( vector3_is_parallel( i, k ), 0 );
  assert_equal_int( vector3_is_parallel( k, j ), 0 );

  assert_equal_int( vector3_is_parallel( i, i ), 1 );
  assert_equal_int( vector3_is_parallel( j, j ), 1 );
  assert_equal_int( vector3_is_parallel( k, k ), 1 );

  assert_equal_int( vector3_is_perpendicular( i, d ), 0 );
  assert_equal_int( vector3_is_perpendicular( j, d ), 0 );
  assert_equal_int( vector3_is_perpendicular( k, d ), 0 );

  assert_equal_int( vector3_is_parallel( i, d ), 0 );
  assert_equal_int( vector3_is_parallel( j, d ), 0 );
  assert_equal_int( vector3_is_parallel( k, d ), 0 );

  assert_equal_double( vector3_angle( i, j ), M_PI_2, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( i, k ), M_PI_2, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( j, k ), M_PI_2, DOUBLE_PRECISION );

  assert_equal_double( vector3_angle( i, d_ij ), M_PI_4, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( i, d_ik ), M_PI_4, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( j, d_ij ), M_PI_4, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( j, d_jk ), M_PI_4, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( k, d_ik ), M_PI_4, DOUBLE_PRECISION );
  assert_equal_double( vector3_angle( k, d_jk ), M_PI_4, DOUBLE_PRECISION );


  assert_equal_double( vector3_dot( i2, i3 ), 6.0, DOUBLE_PRECISION );
  assert_equal_double( vector3_dot( d, d ), 3.0, DOUBLE_PRECISION );

  assert_equal_double( vector3_abs( i ), 1.0, DOUBLE_PRECISION );
  assert_equal_double( vector3_abs( j ), 1.0, DOUBLE_PRECISION );
  assert_equal_double( vector3_abs( k ), 1.0, DOUBLE_PRECISION );
  assert_equal_double( vector3_abs( d ), sqrt(3), DOUBLE_PRECISION );

  vector3_t d_norm = vector3_normalize( d );
  assert_equal_double( VECTOR3_X( d_norm ), 1.0/sqrt(3), DOUBLE_PRECISION );
  assert_equal_double( VECTOR3_Y( d_norm ), 1.0/sqrt(3), DOUBLE_PRECISION );
  assert_equal_double( VECTOR3_Z( d_norm ), 1.0/sqrt(3), DOUBLE_PRECISION );

  vector3_t i3_norm = vector3_normalize( i3 );
  assert_equal_int( vector3_is_equal( i3_norm, i ), 1 );

  assert_equal_int( vector3_is_equal( i3, vector3_multiply_scalar( i, 3.0 ) ),
		    1 );

  assert_equal_int( vector3_is_equal( k, vector3_cross( i, j ) ), 1 );

  return 0;
}
